# in bringup/xeno_raw_control.gui.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import (
    Command,
    FindExecutable,
    PathJoinSubstitution,
    LaunchConfiguration,
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    declared_arguments = [
        DeclareLaunchArgument(
            "gui",
            default_value="true",
            description="Start RViz.",
        )
    ]
    gui = LaunchConfiguration("gui")

    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("xeno_control"), "description/urdf/xeno.urdf.xacro"]
            ),
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="screen",
        parameters=[robot_description],
        arguments=[
            "--ros-args",
            "--params-file",
            PathJoinSubstitution(
                [FindPackageShare("xeno_control"), "config", "controllers.yaml"]
            ),
        ],
    )

    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_state_broadcaster",
            "--controller-manager",
            "/controller_manager",
        ],
        output="screen",
    )

    position_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            "joint_group_position_controller",
            "--controller-manager",
            "/controller_manager",
        ],
        output="screen",
    )

    position_gui_node = Node(
        package="xeno_control",
        executable="position_gui.py",
        condition=IfCondition(gui),
    )

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=[
            "-d",
            PathJoinSubstitution(
                [FindPackageShare("xeno_urdf"), "xeno_urdf/rviz/view_robot.rviz"]
            ),
        ],
        condition=IfCondition(gui),
    )

    return LaunchDescription(
        declared_arguments
        + [
            ros2_control_node,
            joint_state_broadcaster_spawner,
            position_controller_spawner,
            position_gui_node,
            robot_state_publisher,
            rviz_node,
        ]
    )
